#include "WPILib.h"
#include "Commands/Command.h"
#include "Commands/AutonomusMode.h"
#include "Commands/InitializeTeleOp.h"
#include "CommandBase.h"

class BlockingBot : public IterativeRobot {
private:
	Command *autonomousCommand;
	Command *ito;
	//Watchdog *wdog;
	
	virtual void RobotInit() {
		CommandBase::init();
		//SmartDashboard::init();
		autonomousCommand = new AutonomusMode();
		//wdog = new Watchdog();
	}
	
	virtual void AutonomousInit() {
		autonomousCommand->Start();
	}
	
	virtual void AutonomousPeriodic() {
		Scheduler::GetInstance()->Run();
		//wdog->Feed();
	}
	
	virtual void TeleopInit() {
		// This makes sure that the autonomous stops running when
		// teleop starts running. If you want the autonomous to 
		// continue until interrupted by another command, remove
		// this line or comment it out.
		autonomousCommand->Cancel();
		ito = new InitializeTeleOp();
		ito->Start();
	}
	
	virtual void TeleopPeriodic() {
		//wdog->Feed();
		Scheduler::GetInstance()->Run();
	}
};

START_ROBOT_CLASS(BlockingBot);

